#include <sdkeli_ls/sdkeli_ls_common_mockup.h>

namespace sdkeli_ls
{
	CSDKeliLsCommonMockup::CSDKeliLsCommonMockup(CParserBase *pParser) :
		CSDKeliLsCommon(pParser)
	{
		subMockup = nhMockup.subscribe("datagram", 1, &CSDKeliLsCommonMockup::OnMockupMessage, this);
	}

	CSDKeliLsCommonMockup::~CSDKeliLsCommonMockup()
	{
		// Do Nothing...
	}

	int CSDKeliLsCommonMockup::CloseDevice()
	{
		ROS_INFO("Mockup - Close Device");
		return 0;
	}

	int CSDKeliLsCommonMockup::SendDeviceReq(const char *req, std::vector<unsigned char> *resp)
	{
		ROS_ERROR("Mockup - SendDeviceReq(), this should never be called");
		return ExitError;
	}

	int CSDKeliLsCommonMockup::InitDevice()
	{
		ROS_INFO("Mockup - InitDevice()");
		return ExitSuccess;
	}

	int CSDKeliLsCommonMockup::InitScanner()
	{
		ROS_INFO("Mockup - InitScanner()");
		return ExitSuccess;
	}

	int CSDKeliLsCommonMockup::GetDataGram(unsigned char *recvBuffer, int buffer_size, int *length)
	{
		ROS_DEBUG("Mockup - GetDataGram");

		/*Wait until data received...*/
		while(!ptrMsgMockupData)
		{
			if(!ros::ok())
			{
				return ExitError;
			}

			ros::Duration(0.01).sleep();
			ros::spinOnce();
		}

		/*Copy out the message data*/
		std::vector<char> data(ptrMsgMockupData->data.begin(), ptrMsgMockupData->data.end());
		data.push_back('\0');
		*length = ptrMsgMockupData->data.length();

		/*Reset message data structure*/
		ptrMsgMockupData.reset();

		if(buffer_size < (*length + 1))
		{
			ROS_ERROR("Mockup - Memory overflow!");
			return ExitError;
		}

		strncpy(reinterpret_cast<char *>(recvBuffer), &data[0], *length + 1);
		return ExitSuccess;
	}

	void CSDKeliLsCommonMockup::OnMockupMessage(const std_msgs::String::ConstPtr &msg)
	{
		if(ptrMsgMockupData)
		{
			ROS_WARN("Mockup - Dropping mockup datagram message!");
			ptrMsgMockupData.reset();
		}

		ptrMsgMockupData = msg;
	}
}/*sdkeli_ls*/
